#include "TrinamicCommunication.h"

TrinamicCommunication::TrinamicCommunication()
{
    CurrentCoarseFineSetting = 1;
    XZRotationAngle = 0;
    nErrorCount = 0;

    for(int i=0; i<6; i++)
    {
        Polarity[i] = 1;
        LastPositionRotated[i] = -9999.0;
    }
    PivotRadius = 100.0;

    //debug counters
    nMoveCount = 0;
    nCommandCount = 0;
    MicrostepRatio = 6;
}


TrinamicCommunication::~TrinamicCommunication()
{
}

void TrinamicCommunication::Initialize()
{


    //RS232 TESTING
    //READ USB-RS232 GUIDES
    //TRENDNET SEEMS TO WORK BEST

    //OBSERVED--COMM BECOMES UNRELIABLE, USB-RS232 RESET/CYCLE and CABLING HAS NO EFFECT
    //CHANGING CABLE/SIGNALING WORKED TWICE
    //LATEST CASE, CHANGING TO TRENDNET USB CONVERTER WORKED.

    /*
    for(int i = 0 ; i < 10; i++)
    {
    SendCmd(hTrinamicSerial, 1, 136, 0, 0, 0);
    Sleep(500);
    UCHAR Address=0;
    UCHAR Status=0;
    int Value=0;

    DWORD Timeout=GetTickCount();
    while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<1000);

    //DEBUG
    //QString qsTemp;
    printf("add: %d stat: %d val: %d\n", Address, Status, Value);
    }
    */



    qSettings->beginGroup("Trinamic");


    XMotor = qSettings->value("x_motor").toInt();
    YMotor = qSettings->value("y_motor").toInt();
    ZMotor = qSettings->value("z_motor").toInt();
    ThetaMotor = qSettings->value("theta_motor").toInt();
    //qSettings->value("theta_motor").toInt();

    MicronsPerStep[XMotor] = qSettings->value("microns_per_step_x").toFloat();
    MicronsPerStep[YMotor] = qSettings->value("microns_per_step_y").toFloat();
    MicronsPerStep[ZMotor] = qSettings->value("microns_per_step_z").toFloat();

    RelativeModeOrigin[XMotor] = qSettings->value("x_origin").toFloat();
    RelativeModeOrigin[YMotor] = qSettings->value("y_origin").toFloat();
    RelativeModeOrigin[ZMotor] = qSettings->value("z_origin").toFloat();
    RelativeModeOrigin[ThetaMotor] = qSettings->value("theta_origin").toFloat();

    //RotationModeRelativeOrigin[XMotor]

    RadiansPerStep = qSettings->value("radians_per_step_theta").toFloat();
    TrinamicFactor = qSettings->value("trinamic_factor").toFloat();

    bRotationMode = qSettings->value("rotation_mode").toBool();
    bMotorizedRotation = qSettings->value("motorized_rotation").toBool();
    XZRotationAngle = qSettings->value("last_rotation_angle").toFloat();

    PivotRadius = qSettings->value("pivot_radius").toFloat();

    nVelocityPerBin[XMotor] = qSettings->value("velocity_per_bin_x").toInt();
    nVelocityPerBin[YMotor] = qSettings->value("velocity_per_bin_y").toInt();
    nVelocityPerBin[ZMotor] = qSettings->value("velocity_per_bin_z").toInt();

    //6 is 64 microstepping
    //0 is full steps
    MicrostepRatio = qSettings->value("microstep_ratio").toInt();

    if(MicrostepRatio < 0)
    {
        MicrostepRatio = 6;
    }

    //don't touch


    //LM
    //50.0 TPI
    //400 steps per revolution
    //default 64 microstep at finest,
    //estimated 16 - 24 actual resolved steps (must be experimentally verified)

    //LMB
    //2mm per turn
    //400 steps per revolution
    //64 microsteps

    //trinamic factor use the z axis prototyping speed as the standard reference speed
    //replace 200 by the command velocity

    //THESE MUST COME BEFORE SETUPMOTORS
    MicronsPerSecondPerBin = TrinamicFactor * 200 * MicronsPerStep[ZMotor];

    SetupMotors();


    //IMPORTANT
    qSettings->endGroup();



    SetToAbsoluteMode();


    //set velocity settings for all motors
    //with separate settings for z motor
    for(int i=0; i < 3; i++)
    {
        //bins are a joystick notion
        CurrentVelocityBin[i] = 0;
        nVelocityBins[i] = 6;
        nVelocityHysteresis[i] = nVelocityPerBin[i]/2; //purely joystick notion
        nStepsPerStride[i] = 1000;
        nCurrentPositionTarget[i] = 0;
        SetVelocityBin(i,0);
    }
    nStepsPerStride[ZMotor] = 300;

    SetCoarseFineSetting(1);

    //timing loop for 3d connexion
    dwTimeOfLastMove = ::GetTickCount()-100;



}

int TrinamicCommunication::CheckLeftSwitch(int axis)
{
    //type 11 left switch status
    //type 10 right switch status
    //type 9 reference switch status
    SendCmd(hTrinamicSerial, 1, TMCL_GAP, 11, axis, 0);
    //Sleep(30);

    UCHAR Address;
    UCHAR Status;
    int Value;

    DWORD Timeout=GetTickCount();
    while(
          GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<600);

    //DEBUG
    //QString qsTemp;
    //qsTemp.sprintf("add: %d stat: %d val: %d", Address, Status, Value);
    //hMainWindow->ui->RS232Feedback_label->setText(qsTemp);

    if(Status == 100)
    {
        return Value;
    }
    return -2;
}

int TrinamicCommunication::CheckTrinamic()
{
    printf("Finding motor controller...\n");

    UCHAR Address;
    UCHAR Status = 1;
    int Value;

    //try to talk to controller 10 times
    for(int i = 0; i < 10; i++)
    {
        SendCmd(hTrinamicSerial, 1, 136, 1, 0, 0); //ask for binary version
        //SendCmd(hTrinamicSerial, 1, TMCL_MST, 0, axis, 0);



        DWORD Timeout=GetTickCount();
        while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<1000);

        if(Status == 100)
        {
            printf("Found motor controller.\n");
            return 0;
        }
        else
        {
            printf("Failed to find controller, or wrong baudrate.\n");
            return -1;
        }
    }

    return 0;

}

void TrinamicCommunication::SetBaudrate(int baudrate_code)
{
    printf("Setting baudrate...\n");

    UCHAR Address;
    UCHAR Status = 1;
    int Value;

    while(Status != 100)
    {
        SendCmd(hTrinamicSerial, 1, TMCL_SGP, 65, 0, baudrate_code);

        if(1)
        {
            DWORD Timeout=GetTickCount();
            while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<1000);
        }
    }

    if(Status == 100)
    {
        printf("Set baudrate code %d\n", baudrate_code);
    }
    else
    {
        printf("Failed to set baudrate.\n");
    }

}

void TrinamicCommunication::SetupMotors()
{

    //load these settings from somewhere
    //motor parameters for axes

    Polarity[XMotor] = qSettings->value("polarity_x").toInt();
    Polarity[YMotor] = qSettings->value("polarity_y").toInt();
    Polarity[ZMotor] = qSettings->value("polarity_z").toInt();
    Polarity[ThetaMotor] = qSettings->value("polarity_theta").toInt();

    MaximumCurrent[XMotor] = qSettings->value("max_current_x").toInt();
    MaximumCurrent[YMotor] = qSettings->value("max_current_y").toInt();
    MaximumCurrent[ZMotor] = qSettings->value("max_current_z").toInt();
    MaximumCurrent[ThetaMotor] = qSettings->value("max_current_theta").toInt();

    FullStepThreshold[XMotor] = qSettings->value("full_step_threshold_x").toInt();
    FullStepThreshold[YMotor] = qSettings->value("full_step_threshold_y").toInt();
    FullStepThreshold[ZMotor] = qSettings->value("full_step_threshold_z").toInt();
    FullStepThreshold[ThetaMotor] = qSettings->value("full_step_threshold_theta").toInt();

    ReferencingMode[XMotor] = qSettings->value("referencing_mode_x").toInt();
    ReferencingMode[YMotor] = qSettings->value("referencing_mode_y").toInt();
    ReferencingMode[ZMotor] = qSettings->value("referencing_mode_z").toInt();
    ReferencingMode[ThetaMotor] = qSettings->value("referencing_mode_theta").toInt();

    RightLimitSwitchStopDisable[XMotor] = qSettings->value("right_limit_disable_x").toInt();
    RightLimitSwitchStopDisable[YMotor] = qSettings->value("right_limit_disable_y").toInt();
    RightLimitSwitchStopDisable[ZMotor] = qSettings->value("right_limit_disable_z").toInt();
    RightLimitSwitchStopDisable[ThetaMotor] = qSettings->value("right_limit_disable_theta").toInt();

    LeftLimitSwitchStopDisable[XMotor] = qSettings->value("left_limit_disable_x").toInt();
    LeftLimitSwitchStopDisable[YMotor] = qSettings->value("left_limit_disable_y").toInt();
    LeftLimitSwitchStopDisable[ZMotor] = qSettings->value("left_limit_disable_z").toInt();
    LeftLimitSwitchStopDisable[ThetaMotor] = qSettings->value("left_limit_disable_theta").toInt();

    MaximumAcceleration[XMotor] = qSettings->value("max_acceleration_x").toInt();
    MaximumAcceleration[YMotor] = qSettings->value("max_acceleration_y").toInt();
    MaximumAcceleration[ZMotor] = qSettings->value("max_acceleration_z").toInt();
    MaximumAcceleration[ThetaMotor] = qSettings->value("max_acceleration_theta").toInt();

    MaximumSpeed[XMotor] = qSettings->value("max_speed_x").toInt();
    MaximumSpeed[YMotor] = qSettings->value("max_speed_y").toInt();
    MaximumSpeed[ZMotor] = qSettings->value("max_speed_z").toInt();
    MaximumSpeed[ThetaMotor] = qSettings->value("max_speed_theta").toInt();

    SoftStopFlag[XMotor] = qSettings->value("soft_stop_flag_x").toInt();
    SoftStopFlag[YMotor] = qSettings->value("soft_stop_flag_y").toInt();
    SoftStopFlag[ZMotor] = qSettings->value("soft_stop_flag_z").toInt();
    SoftStopFlag[ThetaMotor] = qSettings->value("soft_stop_flag_theta").toInt();


    // previously hardcoded

    //MaximumCurrent[XMotor] = 600;              //set to around 200 (mA)        6
    //MaximumCurrent[YMotor] = 600;              //set to around 200 (mA)        6
    //MaximumCurrent[ZMotor] = 600;              //set to around 200 (mA)        6
    //FullStepThreshold[XMotor] = 100;           //set to some value < 2048      211
    //FullStepThreshold[YMotor] = 100;           //set to some value < 2048      211
    //FullStepThreshold[ZMotor] = 400;           //set to some value < 2048      211
    MicrostepMode[XMotor] = MicrostepRatio;               //6 for 64 microstepping        140
    MicrostepMode[YMotor] = MicrostepRatio;               //6 for 64 microstepping        140
    MicrostepMode[ZMotor] = MicrostepRatio;               //6 for 64 microstepping        140
    MicrostepMode[ThetaMotor] = MicrostepRatio;               //6 for 64 microstepping        140
    ReferenceSearchSpeed[XMotor] = 0;        //zero for full speed           194
    ReferenceSearchSpeed[YMotor] = 0;        //zero for full speed           194
    ReferenceSearchSpeed[ZMotor] = 0;        //zero for full speed           194
    ReferenceSearchSpeed[ThetaMotor] = 0;        //zero for full speed           194
    //ReferencingMode[XMotor] = 2;             //2 for left and right          193
    //ReferencingMode[YMotor] = 2;             //2 for left and right          193
    //ReferencingMode[ZMotor] = 2;             //2 for left and right          193
    //RightLimitSwitchStopDisable[XMotor] = 0; //0 for active limit switch def1     12
    //RightLimitSwitchStopDisable[YMotor] = 0; //0 for active limit switch     12
    //RightLimitSwitchStopDisable[ZMotor] = 0; //0 for active limit switch     12
    //LeftLimitSwitchStopDisable[XMotor] = 0;  //0 for active limit switch     13
    //LeftLimitSwitchStopDisable[YMotor] = 0;  //0 for active limit switch     13
    //LeftLimitSwitchStopDisable[ZMotor] = 0;  //0 for active limit switch     13
    //MaximumAcceleration[XMotor] = 1000;         //max acceleration def 1000     5
    //MaximumAcceleration[YMotor] = 1000;         //max acceleration              5
    //MaximumAcceleration[ZMotor] = 1000;         //max acceleration              5
    //MaximumSpeed[XMotor] = 2000;                //0-2047, default 1000          4
    //MaximumSpeed[YMotor] = 2000;                //0-2047, default 1000          4
    //MaximumSpeed[ZMotor] = 500;                //0-2047, default 1000          4
    //SoftStopFlag[XMotor] = 0;                //0 for failsafe action         149
    //SoftStopFlag[YMotor] = 0;                //0 for failsafe action         149
    //SoftStopFlag[ZMotor] = 0;                //0 for failsafe action         149

    UCHAR Address=0;
    UCHAR Status=0;
    int Value=0;
    DWORD Timeout;
    //bug here if waittime is not well defined
    int waittime= 100;

    for (int i = 0; i < 6; i++ )
    {
        if (i == ThetaMotor)
        {
            if(!bRotationMode)
            {
                //or otherwise do not setup 4th axis (ie for 3 axis board)
                continue;
            }
        }

        //break on unused axes
        if (
                i != XMotor &&
                i != YMotor &&
                i != ZMotor &&
                i != ThetaMotor

                )

        {
            continue;
        }


        /*
        //motor parameters for axes
        int     MaximumCurrent[6];              //set to around 200 (mA)        6
        int     FullStepThreshold[6];           //set to some value < 2048      211
        int     MicrostepMode[6];               //6 for 64 microstepping        140
        int     ReferenceSearchSpeed[6];        //zero for full speed           194
        int     ReferencingMode[6];             //2 for left and right          193
        int     RightLimitSwitchStopDisable[6]; //0 for active limit switch     12
        int     LeftLimitSwitchStopDisable[6];  //0 for active limit switch     13
        int     MaximumAcceleration[6];         //max acceleration              5
        int     MaximumSpeed[6];                //0-2047, default 1000          4
        int     SoftStopFlag[6];                //0 for failsafe action         149
        */


        printf("If these commands take more than 0.1 second each and values are 0, comm is not working.\n");
        SendCmd(hTrinamicSerial, 1, TMCL_SAP, 6, i, MaximumCurrent[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set max current (ax %d) (add %d) (stat %u) (val %d) \n", i, Address, Status, Value);

        SendCmd(hTrinamicSerial, 1, TMCL_SAP, 211, i, FullStepThreshold[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set full step threshold (ax %d) (add %u) (stat %u) (val %d) \n", i, Address, Status, Value);

        SendCmd(hTrinamicSerial, UCHAR(1), TMCL_SAP, 140, i, MicrostepMode[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set microstep mode (ax %d) (add %u) (stat %u) (val %d) \n", i, Address, Status, Value);

        SendCmd(hTrinamicSerial, 1, TMCL_SAP, 194, i, ReferenceSearchSpeed[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set reference search speed (ax %d) (add %u) (stat %u) (val %d) \n", i, Address, Status, Value);

        SendCmd(hTrinamicSerial, 1, TMCL_SAP, 193, i, ReferencingMode[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set reference mode (ax %d) (add %u) (stat %u) (val %d) \n", i, Address, Status, Value);

        SendCmd(hTrinamicSerial, 1, TMCL_SAP, 12, i, RightLimitSwitchStopDisable[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set right limit switch disable (ax %d) address %u) (stat %u) (val %d) \n", i, Address, Status, Value);

        SendCmd(hTrinamicSerial, 1, TMCL_SAP, 13, i, LeftLimitSwitchStopDisable[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set left limit switch disable (ax %d) (add %u) (stat %u) (val %d) \n", i, Address, Status, Value);

        SendCmd(hTrinamicSerial, 1, TMCL_SAP, 5, i, MaximumAcceleration[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set max acceleration (ax %d) (add %u) (stat %u) (val %d) \n", i, Address, Status, Value);

        SendCmd(hTrinamicSerial, 1, TMCL_SAP, 4, i, MaximumSpeed[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set max speed (ax %d) (add %u) (stat %u) (val %d) \n", i, Address, Status, Value);

        SendCmd(hTrinamicSerial, 1, TMCL_SAP, 149, i, SoftStopFlag[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set soft stop flag (ax %d) (add %u) (stat %u) (val %d) \n", i, Address, Status, Value);

        //TODO: check the result, and repeat if the result is no good.
    }

    return;
}

void TrinamicCommunication::RestoreMaximumSpeed()
{

    UCHAR Address;
    UCHAR Status;
    int Value;
    DWORD Timeout;
    //bug here if waittime is not well defined
    int waittime= 1000;
    for(int i= 0; i < 3; i++)
    {
        SendCmd(hTrinamicSerial, 1, TMCL_SAP, 4, i, MaximumSpeed[i]);
        Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
        printf("Trinamic - Set max speed (ax %d) (add %u) (stat %u) (val %d) \n", i, Address, Status, Value);
    }
    return;
}

void TrinamicCommunication::SetMaximumSpeed(int axis, int speed)
{

    UCHAR Address;
    UCHAR Status;
    int Value;
    DWORD Timeout;
    //bug here if waittime is not well defined
    int waittime= 1000;
    SendCmd(hTrinamicSerial, 1, TMCL_SAP, 4, axis, speed);
    Timeout=GetTickCount(); while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<waittime);
    printf("Trinamic - Set max speed (ax %d) (add %u) (stat %u) (val %d) \n", axis, Address, Status, Value);

}

bool TrinamicCommunication::MoveFinished()
{

    double difa, difb, difc;
    difa = abs(CurrentPositionSteps[XMotor] - nCurrentPositionTarget[XMotor]);
    difb  = abs(CurrentPositionSteps[YMotor] - nCurrentPositionTarget[YMotor]);
    difc  = abs(CurrentPositionSteps[ZMotor] - nCurrentPositionTarget[ZMotor]);

    printf("Trinamic: asked if finished with move.  dif:  %f %f %f\n",difa, difb, difc);

    double tolerance = 2.0;
    if(difa < tolerance && difb < tolerance && difc < tolerance)
    {
        //finished move within tolerance
        printf("Trinamic: move finished within tolerance.\n");
        return true;
    }
    else
    {
        printf("Trinamic: move not finished within tolerance.\n");
        return false;
    }

}

void TrinamicCommunication::StopMotors()
{
    SetVelocity(XMotor, 0);
    SetVelocity(YMotor, 0);
    SetVelocity(ZMotor, 0);
    if(bMotorizedRotation) SetVelocity(ThetaMotor, 0);
}

void TrinamicCommunication::MoveToPosition(int axis, double position, bool openloop)
{

    if(bRotationMode)
    {
        //apply forward transformation matrix to X-Z
        //based on origin at absolute (0,0,0)
        //CurrentPositionRotated[XMotor] = CurrentPositionMicrons[XMotor];
        //CurrentPositionRotated[YMotor] = cos(XZRotationAngle)*CurrentPositionMicrons[YMotor] - sin(XZRotationAngle)*CurrentPositionMicrons[ZMotor];
       // CurrentPositionRotated[ZMotor] = sin(XZRotationAngle)*CurrentPositionMicrons[YMotor] + cos(XZRotationAngle)*CurrentPositionMicrons[ZMotor];

        double target[3] = {
            CurrentPositionRotated[0],
            CurrentPositionRotated[1],
            CurrentPositionRotated[2]
        };

        double unrotatedtarget[3];

        target[axis] = position;

        unrotatedtarget[XMotor] = target[XMotor];
        unrotatedtarget[YMotor] = cos(XZRotationAngle)*target[YMotor] + sin(XZRotationAngle)*target[ZMotor];
        unrotatedtarget[ZMotor] = -1*sin(XZRotationAngle)*target[YMotor] + cos(XZRotationAngle)*target[ZMotor];

        for(int i = 0; i < 3; i++)
        {
            nCurrentPositionTarget[i] = unrotatedtarget[i]/MicronsPerStep[i];
            SetTargetPosition(i, nCurrentPositionTarget[i], openloop);
        }

    }
    else
    {

        //this line implements a position seek with zero lag, throwing away late actions.
        int position_in_steps = position/MicronsPerStep[axis];
        nCurrentPositionTarget[axis] = position_in_steps;
        SetTargetPosition(axis, nCurrentPositionTarget[axis], openloop);
    }

    return;
}

void TrinamicCommunication::MoveToPosition(int axis1, double position1,
                                              int axis2, double position2,
                                              int axis3, double position3,
                                              bool openloop)
{


    if(bRotationMode)
    {
        //apply forward transformation matrix to X-Z
        //based on origin at absolute (0,0,0)
        //CurrentPositionRotated[XMotor] = CurrentPositionMicrons[XMotor];
        //CurrentPositionRotated[YMotor] = cos(XZRotationAngle)*CurrentPositionMicrons[YMotor] - sin(XZRotationAngle)*CurrentPositionMicrons[ZMotor];
       // CurrentPositionRotated[ZMotor] = sin(XZRotationAngle)*CurrentPositionMicrons[YMotor] + cos(XZRotationAngle)*CurrentPositionMicrons[ZMotor];

        double target[3] = {
            CurrentPositionRotated[0],
            CurrentPositionRotated[1],
            CurrentPositionRotated[2]
        };

        double unrotatedtarget[3];

        target[axis1] = position1;
        target[axis2] = position2;
        target[axis3] = position3;

        unrotatedtarget[XMotor] = target[XMotor];
        unrotatedtarget[YMotor] = cos(XZRotationAngle)*target[YMotor] + sin(XZRotationAngle)*target[ZMotor];
        unrotatedtarget[ZMotor] = -1*sin(XZRotationAngle)*target[YMotor] + cos(XZRotationAngle)*target[ZMotor];

        for(int i = 0; i < 3; i++)
        {
            nCurrentPositionTarget[i] = unrotatedtarget[i]/MicronsPerStep[i];
            SetTargetPosition(i, nCurrentPositionTarget[i], openloop);
        }

    }
    else
    {
        //this line implements a position seek with zero lag, throwing away late actions.
        int position_in_steps1 = position1/MicronsPerStep[axis1];
        int position_in_steps2 = position2/MicronsPerStep[axis2];
        int position_in_steps3 = position3/MicronsPerStep[axis3];
        nCurrentPositionTarget[axis1] = position_in_steps1;
        nCurrentPositionTarget[axis2] = position_in_steps2;
        nCurrentPositionTarget[axis3] = position_in_steps3;
        SetTargetPosition(axis1, nCurrentPositionTarget[axis1], openloop);
        SetTargetPosition(axis2, nCurrentPositionTarget[axis2], openloop);
        SetTargetPosition(axis3, nCurrentPositionTarget[axis3], openloop);
    }

    return;
}

void TrinamicCommunication::MoveToAngleRelative(double radians)
{
    SetTargetPosition(ThetaMotor, CurrentPositionSteps[ThetaMotor]+int(radians/RadiansPerStep));
    return;
}

void TrinamicCommunication::MoveDeltaPosition(int axis1, double position1,
                                              int axis2, double position2,
                                              int axis3, double position3,
                                              bool openloop)
{

    MoveToPosition(
                axis1, CurrentPositionRotated[axis1]+position1,
                axis2, CurrentPositionRotated[axis2]+position2,
                axis3, CurrentPositionRotated[axis3]+position3,
                   openloop);
    return;
}


void TrinamicCommunication::TakeStrides(int axis, int nStrides, bool openloop)
{

    //current position must be asked for at least once before calling this function

    /*
    //this line implements a position seek with zero lag, throwing away late actions.
    nCurrentPositionTarget[axis] = CurrentPositionSteps[axis];

    //set the target position to be the desired position, plus some steps.

    //modulated by coarse fine settings, and steps per stride

    //unified architecture for angle? (fully differential)
    nCurrentPositionTarget[axis] +=
            nStrides*nStepsPerStride[axis] /
            (CurrentCoarseFineSetting*CurrentCoarseFineSetting*CurrentCoarseFineSetting);
            */

    double dist = nStrides*nStepsPerStride[axis]*MicronsPerStep[axis] /
            (CurrentCoarseFineSetting*CurrentCoarseFineSetting*CurrentCoarseFineSetting);

    //rotationmode safe
    //check the distance from the last successful move command
    //if we haven't moved, ignore this potentially redundant command
    //case: first move, we are stationary.
    if(abs(CurrentPositionRotated[axis] - LastPositionRotated[axis]) > abs(dist/10))
    {
        //do this only if we are some distance from the last stride
        //sparsification for shuttle?
        //I guess, filters repeat commands.
        MoveToPosition(axis, CurrentPositionRotated[axis] + dist, openloop);
        LastPositionRotated[axis] = CurrentPositionRotated[axis];
    }
    else
    {
        //printf("rotationmode safety\n");
    }

    //SetTargetPosition(axis, nCurrentPositionTarget[axis]);

    return;
}

void TrinamicCommunication::SetTargetPosition(int axis, int position, bool openloop)
{

    position = position*Polarity[axis];

    SendCmd(hTrinamicSerial, 1, TMCL_MVP, 0, axis, position);

    UCHAR Address;
    UCHAR Status;
    int Value;
    DWORD Timeout=GetTickCount();

    if(!openloop)
    {
        while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<1000);
    }

    //DEBUG
    QString qsTemp;
    qsTemp.sprintf("add: %d stat: %d val: %d", Address, Status, Value);

    printf("Tri-setpos (ax %d) (%08.2f um/s) (pos %05d)\n",
           axis, 0.0, position);

    return;
}

/**
 * @brief Generic function for keyboard and all input devices, to set current velocity.
 *
 * @param axis
 * @param bin
 */
void TrinamicCommunication::SetVelocityBin(int axis, int bin, bool openloop)
{

    float coarsefine = ( CurrentCoarseFineSetting * CurrentCoarseFineSetting );
    //unique velocity mode treatment for hardware level angle
    //steps the rotator, without any parameterization
    if (axis == ThetaMotor)
    {
        //0.0225 deg per step URS50BPP
        //64x microstepping
        //0.0255 / 64 * 2.0*PI / 360.0

        int velo = 1000*bin/coarsefine;
        SetVelocity(ThetaMotor,velo);
        printf("Set theta velo (ax %d) (val %05d)\n", ThetaMotor, velo);
        return;
    }


    if (CurrentVelocityBin[axis] == bin)
    {
        //no work needed, don't create rs232 traffic
        return;
    }

    //...new velocity

    CurrentVelocityBin[axis] = bin;

    //these boundaries are always positive
    CurrentVelocityBinBoundaryLow[axis] = nVelocityPerBin[axis]*abs(bin);
    CurrentVelocityBinBoundaryHigh[axis] = nVelocityPerBin[axis]*(abs(bin)+1);

    //need to translate to true axis here*
    //also scaling based on coarse fine here?


    //for normal translation
    if(!bRotationMode)
    {

        float amplitude = MicronsPerSecondPerBin / TrinamicFactor / MicronsPerStep[axis] / coarsefine;
        amplitude = MaximumSpeed[axis]/coarsefine/3;
        int value = bin*amplitude;
        SetVelocity(axis, value , openloop);

        printf("Tri-setvelo (ax %d) (%08.2f um/s) (val %05d)\n",
               axis, MicronsPerSecondPerBin / coarsefine * bin, value);
    }
    else
        //for rotated translation
    {

        //inverse transformation to find step ratios

        float originalbins[3] = {0.0, 0.0, 0.0};
        float unrotatedbins[3];

        //assign bin to be modified
        originalbins[axis] = float(bin)/3.0;

        //do rotation
        unrotatedbins[XMotor] = originalbins[XMotor];
        unrotatedbins[YMotor] = cos(XZRotationAngle)*originalbins[YMotor] + sin(XZRotationAngle)*originalbins[ZMotor];
        unrotatedbins[ZMotor] = -1*sin(XZRotationAngle)*originalbins[YMotor] + cos(XZRotationAngle)*originalbins[ZMotor];

        //float mag = MaximumSpeed[axis]/coarsefine;
        for(int i=0; i < 3; i++)
        {
            //old amp
            float amplitude = MicronsPerSecondPerBin / TrinamicFactor / MicronsPerStep[i] / coarsefine;

            //test amp
            //make amplitude relative to x motor, ie in rotation mode
            //max speed for all is max speed for slowest motor
            amplitude = MaximumSpeed[XMotor]*MicronsPerStep[XMotor]/MicronsPerStep[i]/coarsefine;

            int value = unrotatedbins[i]*amplitude;

            SetVelocity(i, value , openloop);

            printf("Tri-setvelo (ax %d) (%08.2f um/s) (val %05d)\n",
                   i, MicronsPerSecondPerBin / coarsefine * unrotatedbins[i], value);
        }
    }


    //debug, count number of commands
    nCommandCount++;
    //hMainWindow->ui->debug_ry->setText(QString::number(nCommandCount));



}

/**
 * @brief Intermediate function for 3d connexion or joystick devices.
 * Checks to see if the speed of whatever input device puts us into a new bin,
 * if so, calls SetVelocityBin
 *
 * @param axis
 * @param speed
 */
void TrinamicCommunication::CheckVelocityBin(int axis, int speed)
{
    bool metthreshold = false;
    //check to see that we meet threshold
    //zeroth case, velocity bin is zero
    if(CurrentVelocityBin[axis] == 0)
    {
        if(abs(speed) > CurrentVelocityBinBoundaryHigh[axis] + nVelocityHysteresis[axis])
        {
            metthreshold = true;
            //in a new bin
        }
        else
        {
            //we remain in bin 0
        }
    }
    //check to see that we are in lower speed bin
    else if(abs(speed) < CurrentVelocityBinBoundaryLow[axis])
    {
        //moved to a lower speed bin
        metthreshold = true;
    }
    else if(abs(speed) > CurrentVelocityBinBoundaryHigh[axis] + nVelocityHysteresis[axis])
    {
        //moved to a higher speed bin
        metthreshold = true;
    }

    if(metthreshold)
    {
        int nextbin = abs(speed)/nVelocityPerBin[axis];
        if (speed < 0) nextbin *= -1; //change parity
        SetVelocityBin(axis,nextbin);

        //for a multi-axis event, let 50ms pass
        //Sleep(50);

        //Also ask for current position
        int position = AskForPosition(axis);
        CurrentPositionSteps[axis] = position;
        CurrentPositionMicrons[axis] = position*MicronsPerStep[axis];

        //hMainWindow->ui->lcdNumber->display(CurrentPositionMicrons[axis]);
        //hMainWindow->TrinamicUpdateLCD();

        //DEBUG
        //QString qsTemp;
        //qsTemp.sprintf("setvelocitybin axis: %d bin: %d", axis, nextbin);

        //hMainWindow->TrinamicUpdateDebug();
        //hMainWindow->ui->RS232Output_label->setText(qsTemp);
    }

    return;
}

void TrinamicCommunication::SetCoarseFineSetting(int setting)
{
    //StopMotors();
    CurrentCoarseFineSetting = setting;
    for(int i=0; i<3; i++)
    {
        //stops motors and sets bin to zero
        SetVelocityBin(i,0);
    }
}

void TrinamicCommunication::SetToRelativeMode()
{
    bRelativeMode = true;
    return;
}

void TrinamicCommunication::SetRelativeModeOrigin()
{
    for (int i = 0; i < 3; i++ )
    {
        RelativeModeOrigin[i] = CurrentPositionMicrons[i];
        RotationModeRelativeOrigin[i] = CurrentPositionRotated[i];
    }

    return;
}

void TrinamicCommunication::SetMotorizedRotationOrigin()
{
    RelativeModeOrigin[ThetaMotor] = CurrentPositionMicrons[ThetaMotor];
}

void TrinamicCommunication::SetToAbsoluteMode()
{
    bRelativeMode = false;
    return;
}



/**
 * @brief This function is mostly for handling very specific input from the 3D connexion joystick
 *
 * @param motionData
 */
void TrinamicCommunication::ProcessControllerMove(std::vector<float> &motionData)
{

    DWORD dwNow = ::GetTickCount();                     // Current time;
    DWORD dwElapsed = dwNow - dwTimeOfLastMove; // Elapsed time since we were last here


    if (0 == dwElapsed) {
            dwElapsed = 10;                    // System timer resolution
    } else {
            if (dwElapsed < 0) {
                    dwElapsed = ~dwElapsed+1;
            }
            if (dwElapsed<1) {
                    dwElapsed=1;
            } else if (dwElapsed > 500) {
                    // Check for wild numbers because the device was removed while sending data
                    //dwElapsed = 10;
            }
    }


    //Now we have the time since the last move.
    //Send a RS232 command appropriately
    //Recieve a RS232 acknowledgement?

    //translate motionData[xindex] to XMotor motion command


    QString qsTemp;


    //debug
    qsTemp.sprintf("elapsed %d", dwElapsed);
    //hui->RS232Output_label->setText(qsTemp);


    //skip every 5 moves
    nMoveCount++;
    nMoveCount %= 30;

    //deadzone considerations
    //float deadzone = 10;
    //bool dead[3];
    //for(int i = 0; i < 3; i++) dead[i] = abs(motionData[i]) <= deadzone;


    //if(dwElapsed > 200 || abs(motionData[0]) < 10)
    //if(nMoveCount == 0 || dead[0] || dead[1] || dead[2])
    if(1) //process every move
    {
        //only record a time if we successfully moved
        dwTimeOfLastMove = dwNow;

        int speed[3];

        for(int i = 0; i < 3; i++)
        {
            //translate the "xyz" speed and "angular rate" of the 3d connexion to step rate
            speed[i] = (int)motionData[i]/3;

            //limit the data range of the speed
            speed[i] = max(speed[i], -1000);
            speed[i] = min(speed[i], 1000);

            CheckVelocityBin(i, speed[i]);
        }

    }

    return;
}



void TrinamicCommunication::PositionQuery()
{
    //called every 200 ms or so, based on mTimerPosition
    //single threadedness ensures no other communication will be interrupted

    int position[6];
    double position_microns[6];

    //get position
    //multiply by microns per step to get microns
    //assign the position to class variable
    position[XMotor] = AskForPosition(XMotor);
    position[YMotor] = AskForPosition(YMotor);
    position[ZMotor] = AskForPosition(ZMotor);
    position_microns[XMotor] = position[XMotor]*MicronsPerStep[XMotor];
    position_microns[YMotor] = position[YMotor]*MicronsPerStep[YMotor];
    position_microns[ZMotor] = position[ZMotor]*MicronsPerStep[ZMotor];
    CurrentPositionMicrons[XMotor] = position_microns[XMotor];
    CurrentPositionMicrons[YMotor] = position_microns[YMotor];
    CurrentPositionMicrons[ZMotor] = position_microns[ZMotor];
    CurrentPositionSteps[XMotor] = position[XMotor];
    CurrentPositionSteps[YMotor] = position[YMotor];
    CurrentPositionSteps[ZMotor] = position[ZMotor];

    //if motorized rotation, then all rotation angle information
    //is calculated in this block, including zero reference.
    //otherwise, it is set externally/manually
    if(bMotorizedRotation)
    {
        position[ThetaMotor] = AskForPosition(ThetaMotor);
        CurrentPositionSteps[ThetaMotor] = position[ThetaMotor];
        CurrentPositionMicrons[ThetaMotor] = RadiansPerStep*CurrentPositionSteps[ThetaMotor];
        double integer;

        //relative rotation zero is taken into account
        //via "set zero", calculated on absolute scale
        double relative_angle = CurrentPositionMicrons[ThetaMotor] - RelativeModeOrigin[ThetaMotor];
        XZRotationAngle = modf(relative_angle/(2*PI), &integer);

        //XZRotation is -1 to 1
        //Final XZRotation is 0 to 2PI
        XZRotationAngle = modf(XZRotationAngle+1.0, &integer)*(2*PI);

    }


    //apply forward transformation matrix to X-Z
    //based on origin at absolute (0,0,0)
    CurrentPositionRotated[XMotor] = CurrentPositionMicrons[XMotor];

    CurrentPositionRotated[YMotor] = cos(XZRotationAngle)*CurrentPositionMicrons[YMotor] - sin(XZRotationAngle)*CurrentPositionMicrons[ZMotor];
    CurrentPositionRotated[ZMotor] = sin(XZRotationAngle)*CurrentPositionMicrons[YMotor] + cos(XZRotationAngle)*CurrentPositionMicrons[ZMotor];




    return;
}

void TrinamicCommunication::MoveThetaRadius(double radians)

{

    double z_polarity = 1;
    double y_polarity = 1;
    double rad_polarity = 1;
    double radius_microns = PivotRadius*1000.0;

    //example
    //at zero degrees
    //for 1 radian change
    //y pos is max and pos/neg (sin(xzrotation)*radius*z_polarity)
    //z pos is minimum and neg/neg (-cos(xzrotation)*radius*z_polarity)

    //give existing angle, find pos on circle
    double old_angle = XZRotationAngle;
    double old_y = sin(old_angle)*radius_microns*y_polarity;
    double old_z =  - (1 - cos(old_angle))*radius_microns*z_polarity;

    //given new angle, find pos on circle
    double new_angle = XZRotationAngle + radians;
    double new_y = sin(new_angle)*radius_microns*y_polarity;
    double new_z = - (1 - cos(new_angle))*radius_microns*z_polarity;

    //we are at the cartesian old circle coordinates. subtract old to get back to origin
    //then add new coordinates
    double mov_x = CurrentPositionMicrons[XMotor];
    double mov_y = CurrentPositionMicrons[YMotor] + new_y - old_y;
    double mov_z = CurrentPositionMicrons[ZMotor] + new_z - old_z;

    //make the move
    this->MoveToAngleRelative(radians*rad_polarity);

    bRotationMode = false;
    SetMaximumSpeed(XMotor,1000);
    SetMaximumSpeed(YMotor,1000);
    SetMaximumSpeed(ZMotor,300);
    this->MoveToPosition(
                XMotor, mov_x,
                YMotor, mov_y,
                ZMotor, mov_z,
                false );
    //RestoreMaximumSpeed();
    bRotationMode = true;

    printf("Moving theta radius (x,y,z) (%f,%f,%f) \n",
           mov_x,
           mov_y,
           mov_z);

    return;

}

/**
 * @brief Sends RS232 command to get position, and returns integer position in abs microsteps
 *
 * @param axis
 * @return int
 */
int TrinamicCommunication::AskForPosition(int axis)
{
    SendCmd(hTrinamicSerial, 1, TMCL_GAP, 1, axis, 0);
    //Sleep(30);

    UCHAR Address;
    UCHAR Status;
    int Value;

    DWORD Timeout=GetTickCount();
    while(
          GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<600);

    //DEBUG
    //QString qsTemp;
    //qsTemp.sprintf("add: %d stat: %d val: %d", Address, Status, Value);
    //hMainWindow->ui->RS232Feedback_label->setText(qsTemp);

    if(Status == 100)
    {
        return Value*Polarity[axis];
    }
    return -2;

}

/**
 * @brief Sends RS232 command for velocity change, and gets status back.
 *
 * @param axis
 * @param stepRate
 */
void TrinamicCommunication::SetVelocity(int axis, int stepRate, bool openloop)
{
    //moving right or left or stop?
    stepRate = stepRate*Polarity[axis];
    bool bMoveRight = (stepRate < 0);
    bool bMoveLeft = !bMoveRight;
    stepRate = abs(stepRate);
    stepRate = min(stepRate, MaximumSpeed[axis]);

    if (bMoveRight)
    {
        SendCmd(hTrinamicSerial, 1, TMCL_ROR, 0, axis, stepRate);
    }
    else if (bMoveLeft)
    {
        SendCmd(hTrinamicSerial, 1, TMCL_ROL, 0, axis, stepRate);
    }
    else if (stepRate == 0)
    {
        SendCmd(hTrinamicSerial, 1, TMCL_MST, 0, axis, 0);
    }

    if(!openloop)
    {

        UCHAR Address;
        UCHAR Status;
        int Value;

        DWORD Timeout=GetTickCount();
        while(GetResult(hTrinamicSerial, &Address, &Status, &Value)==TMCL_RESULT_NOT_READY && abs(int(GetTickCount()-Timeout))<1000);
    }

    //DEBUG
    //QString qsTemp;

    //printf("add: %d stat: %d val: %d\n", 1, 100, stepRate);

    //hMainWindow->ui->RS232Feedback_label->setText(qsTemp);
    //hMainWindow->TrinamicUpdateDebug();


    return;
}



//Send a binary TMCL command
//e.g.  SendCmd(ComHandle, 1, TMCL_MVP, MVP_ABS, 1, 50000);   will be MVP ABS, 1, 50000 for a module with address 1
//Parameters: Handle: Handle of the serial port (returned by OpenRS232).
//            Address: address of the module (factory default is 1).
//            Command: the TMCL command (see the constants at the begiining of this file)
//            Type:    the "Type" parameter of the TMCL command (set to 0 if unused)
//            Motor:   the motor number (set to 0 if unused)
//            Value:   the "Value" parameter (depending on the command, set to 0 if unused)
void TrinamicCommunication::SendCmd(HANDLE Handle, UCHAR Address, UCHAR Command, UCHAR Type, UCHAR Motor, INT Value)
{
        UCHAR TxBuffer[9];
        DWORD BytesWritten;
        int i;

        TxBuffer[0]=Address;
        TxBuffer[1]=Command;
        TxBuffer[2]=Type;
        TxBuffer[3]=Motor;
        TxBuffer[4]=Value >> 24;
        TxBuffer[5]=Value >> 16;
        TxBuffer[6]=Value >> 8;
        TxBuffer[7]=Value & 0xff;
        TxBuffer[8]=0;
        for(i=0; i<8; i++)
                TxBuffer[8]+=TxBuffer[i];

        //Send the datagram
        if (!WriteFile(Handle, TxBuffer, 9, &BytesWritten, NULL))
        {
            printf("Trinamic - Send command failed at OS level - probably comm port not configured correctly\n");
        }
}


//Read the result that is returned by the module
//Parameters: Handle: handle of the serial port, as returned by OpenRS232
//            Address: pointer to variable to hold the reply address returned by the module
//            Status: pointer to variable to hold the status returned by the module (100 means okay)
//            Value: pointer to variable to hold the value returned by the module
//Return value: TMCL_RESULT_OK: result has been read without errors
//              TMCL_RESULT_NOT_READY: not enough bytes read so far (try again)
//              TMCL_RESULT_CHECKSUM_ERROR: checksum of reply packet wrong
UCHAR TrinamicCommunication::GetResult(HANDLE Handle, UCHAR *Address, UCHAR *Status, int *Value)
{
    UCHAR RxBuffer[9], Checksum;
    DWORD Errors, BytesRead;
    COMSTAT ComStat;
    int i;

    //Check if enough bytes can be read
    ClearCommError(Handle, &Errors, &ComStat);

    if(ComStat.cbInQue>8)
    {
        //Receive
        ReadFile(Handle, RxBuffer, 9, &BytesRead, NULL);

        Checksum=0;
        for(i=0; i<8; i++)
                Checksum+=RxBuffer[i];

        if(Checksum!=RxBuffer[8])
        {
            //checksum doesn't match
            //QDebug(QtDebugMsg) << "TMCL_RESULT_CHECKSUM_ERROR" << endl;
            printf("Motor comm checksum error.  Is your motor box turned on?\r");
            nErrorCount++;
            return TMCL_RESULT_CHECKSUM_ERROR;
        }

        *Address=RxBuffer[0];
        *Status=RxBuffer[2];
        *Value=(RxBuffer[4] << 24) | (RxBuffer[5] << 16) | (RxBuffer[6] << 8) | RxBuffer[7];
    }
    else
    {
        //there was less than 8 bytes in the rs232 queue
        //printf("Trinamic - Waiting for reply %d bytes\n", ComStat.cbInQue);
        return TMCL_RESULT_NOT_READY;
    }


    //everything was ok
    nErrorCount = 0;
    return TMCL_RESULT_OK;
}
